function particles = initialize_particles(N, initial_pose, initial_noise)
% 初始化粒子（改进版：支持指定初始位姿）
% 输入: N-粒子数量
%       initial_pose-初始位姿[x;y;theta]（可选，默认[0;0;0]）
%       initial_noise-初始噪声[σx;σy;σθ]（可选，默认[0;0;0]）
% 输出: particles-粒子数组

    % 默认参数
    if nargin < 2 || isempty(initial_pose)
        initial_pose = [0; 0; 0];
    end
    
    if nargin < 3 || isempty(initial_noise)
        initial_noise = [0; 0; 0];  % 无噪声初始化
    end
    
    particles = struct('pose', {}, 'weight', {}, 'landmarks', {}, 'landmark_ids', {});
    
    for i = 1:N
        % 在初始位姿周围添加小的随机扰动（如果指定了噪声）
        noise = initial_noise .* randn(3, 1);
        particles(i).pose = initial_pose + noise;
        particles(i).pose(3) = wrapToPi(particles(i).pose(3));  % 角度归一化
        
        particles(i).weight = 1/N;  % 均匀权重
        particles(i).landmarks = [];  % 路标EKF列表（结构体数组）
        particles(i).landmark_ids = [];  % 已观测路标ID
    end
end

